# coding : utf-8

"""
motion model of vehicle
"""

import math
import numpy as np

import carla

from agents.tools.misc import compute_distance, get_speed

class MotionStatus:
    def __init__(self, vehicle, max_steer_speed, dt=0.01):
        """
        @param vehicle : carla.Vehicle
        @param max_steer_speed : degree/s
        @param dt : s
        """
        self._vehicle = vehicle
        self._location = vehicle.get_location()
        self._yaw = vehicle.get_transform().rotation.yaw # 车身朝向
        self._max_steer_speed = max_steer_speed
        self._dt = dt

    def get_vehicle_axis(self, vehicle):
        """
        获取车辆轴长（前后轴距）
        """
        wheels  =  vehicle.get_physics_control().wheels
        return compute_distance(wheels[0].position, wheels[2].position) / 100.0
    
    def  get_max_steer_angle(self):
        return self._vehicle.get_physics_control().wheels[0].max_steer_angle / 2.0

    def next_status(self):
        """
        获取车辆下一状态（自行车模型：x，y，yaw）
        """
        speed = get_speed(self._vehicle)
        phy = math.fabs(self._vehicle.get_control().steer * 
            self._vehicle.get_physics_control().wheels[0].max_steer_angle / 2.0) # 前轮转角
        axis_length =  self.get_vehicle_axis(self._vehicle)
        self._location.x += speed * math.cos(math.radians(self._yaw)) * self._dt
        self._location.y += speed * math.sin(math.radians(self._yaw)) * self._dt
        self._yaw += speed * math.tan(math.radians(phy)) / axis_length * self._dt
        pass

    def get_max_dappa(self):
        """
        获取当前状态下的可适应的最大曲率变化率
        """
        speed = get_speed(self._vehicle)
        max_dappa = 0
        
        return max_dappa 

if __name__ == "__main__":
    import matplotlib.pyplot as plt

    def GetWave(x0, y0, k0, x1, y1, k1):
        x_matrix = [[x0**3, x0**2, x0, 1], 
                                [3*x0**2, 2*x0, 1, 0], 
                                [x1**3, x1**2, x1, 1],
                                [3*x1**2, 2*x1, 1, 0]]
        y_matrix = [[y0], 
                                [k0],
                                [y1],
                                [k1]]
        p = np.linalg.solve(x_matrix, y_matrix)
        x = np.arange(x0, x1+(x1-x0)/100.0, (x1-x0)/100.0)
        y = [p[0]*i**3+p[1]*i**2+p[2]*i+p[3] for i in x]
        return zip(x, y)

    wave = GetWave(0, 0, 45, 20, 20, 90)
    x, y = zip(*wave)
    plt.plot(x,y)
    plt.show()